﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using TinyROS.Messages;
using TinyROS.Exceptions;
using System.Collections;

namespace TinyROS
{
    public enum SensorProcessorType
    {
        DiffcarMotorIntegrator,
        MagnetometerConverter
    }
    public class SensorFusionInfo : List<(string TopicName, SensorProcessorType ProcessorType)>
    {

    }
    public class SensorFusionNode : Node
    {
        private Publisher<CarStatusMessage> CarStatusPublisher;
        private List<Subscriber> SensorSubscribers = new();
        private Dictionary<SensorProcessorType,List<SensorProcessor>> SensorProcessors = new();
        private Dictionary<Publisher, string> pubedTopicName = new();
        private Dictionary<Subscriber, string> subedTopicName = new();
        public override Dictionary<Publisher, string> PublishedTopicName { get => this.pubedTopicName; }
        public override Dictionary<Subscriber, string> SubscribedTopicName { get => this.subedTopicName; }
        private bool running = false;
        public override bool IsRunning => this.running;

        public SensorFusionNode(string carStatusPubName, SensorFusionInfo sensorFusionInfo)
        {
            this.CarStatusPublisher = new();
            this.PublishedTopicName.Add(this.CarStatusPublisher, carStatusPubName);           
            foreach(var info in sensorFusionInfo)
            {
                if (!this.SensorProcessors.ContainsKey(info.ProcessorType))
                {
                    SensorProcessors.Add(info.ProcessorType, new());
                }               
                switch (info.ProcessorType)
                {
                    case SensorProcessorType.DiffcarMotorIntegrator:
                        var dmi = new DiffcarMotorIntegrator();
                        var dvSubscriber = new Subscriber<DiffcarVelocityMessage>(dmi.DiffcarVelocityMessageUpdatedHandler);
                        this.SensorProcessors[info.ProcessorType].Add(dmi);
                        this.SubscribedTopicName.Add(dvSubscriber, info.TopicName);
                        break;
                }
            }
        }
        public override void Init(SystemCore master)
        {
            throw new NotImplementedException();
        }

        public override void Run()
        {
            throw new NotImplementedException();
        }

        public override void Stop()
        {
            throw new NotImplementedException();
        }
    }
    public abstract class SensorProcessor
    {

    }
    public class SensorImuNode : Node
    {
        public override bool IsRunning => throw new NotImplementedException();

        public override Dictionary<Publisher, string> PublishedTopicName => throw new NotImplementedException();

        public override Dictionary<Subscriber, string> SubscribedTopicName => throw new NotImplementedException();

        public override void Init(SystemCore master)
        {
            throw new NotImplementedException();
        }

        public override void Run()
        {
            throw new NotImplementedException();
        }

        public override void Stop()
        {
            throw new NotImplementedException();
        }
    }

    public class DiffcarMotorIntegrator : SensorProcessor
    {
        private DiffcarVelocityMessage last;
        private long lastTick;
        private float lastV;
        private float I;
        public DiffcarMotorIntegrator()
        {
            this.I = 0.0f;
        }
        public float Mile { get => this.I; }
        public void DiffcarVelocityMessageUpdatedHandler(DiffcarVelocityMessage msg)
        {
            this.UpdateMile(msg);
        }
        private void UpdateMile(DiffcarVelocityMessage m)
        {
            if(m == null)
            {
                throw new MessageTypeException();
            }
            else
            {
                if(this.last == null)
                {
                    this.last = m;
                }
                else
                {
                    var v1 = this.lastV;
                    var v2 = this.GetLinear(m);
                    var t1 = this.lastTick;
                    var t2 = this.GetTick(m);
                    var dt = new TimeSpan(t2 - t1);
                    var dts = dt.TotalSeconds;
                    this.I += (v1 + v2) * (float)dts / 2;
                    this.last = m;
                    this.lastV = v2;
                    this.lastTick = t2;
                }
            }
        }
        private long GetTick(DiffcarVelocityMessage msg)
        {
            return msg.Header.Time.Ticks;   
        }
        private float GetLinear(DiffcarVelocityMessage msg)
        {
            return (msg.Value.Left + msg.Value.Right) / 2;
        }

    }



}
